/*******************************************************************************
Transmission type
00:	��ѭ��ͬ����
01��ѭ��ͬ����
FC��Զ��ͬ����
FD��Զ���첽��
FE���첽���������ض��¼���
FF���첽���豸��Э���ض��¼�
********************************************************************************/
//#define TRANS_EVERY_N_SYNC(n) (n) /*n = 1 to 240 */
//#define TRANS_SYNC_ACYCLIC    0    /* Trans after reception of n SYNC. n = 1 to 240 */
//#define TRANS_SYNC_MIN        1    /* Trans after reception of n SYNC. n = 1 to 240 */
//#define TRANS_SYNC_MAX        240  /* Trans after reception of n SYNC. n = 1 to 240 */
//#define TRANS_RTR_SYNC        252  /* Transmission on request */
//#define TRANS_RTR             253  /* Transmission on request */
//#define TRANS_EVENT_SPECIFIC  254  /* Transmission on event */
//#define TRANS_EVENT_PROFILE   255  /* Transmission on event */

#include "maxsine_motor.h"
#include "main.h"
#include "stdlib.h"
#include "eeprom.h"
#include "USBCdc.h"
#include "Leds.h"
#include "CallBack.h"
#define DBG_TAG "maxsine_motor"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define motor_od	&Maxsine_Motor_OD_Data

#define TO_HOME_TORQUE			-200
#define	ASK_SERVO_TRY_TIMES		1000
#define ASK_SERVO_INTERVAL		500

#define TPDO_SYN_CYCLE			0x00000000			// ��λus
#define TPDO_TRANS_TYPE			TRANS_EVENT_SPECIFIC
#define TPDO_TRANS_EVENT_TIME	5					// �ŷ�״̬�ϱ������ʵ��ʱ����15ms
#define TPDO_STATUS_WORD_TIME	100					// ״̬���ϱ����
#define DRIVE_MODE				CST_MODE

#define DICT_UINT8(i,s,d)		{.index = i, .subIndex = s, .dataType = uint8,	.count = 1, .data = d}
#define DICT_INT8(i,s,d)		{.index = i, .subIndex = s, .dataType = int8,	.count = 1, .data = d}
#define DICT_UINT16(i,s,d)		{.index = i, .subIndex = s, .dataType = uint16,	.count = 2, .data = d}
#define DICT_INT16(i,s,d)		{.index = i, .subIndex = s, .dataType = int16,	.count = 2, .data = d}
#define DICT_UINT32(i,s,d)		{.index = i, .subIndex = s, .dataType = uint32,	.count = 4, .data = d}
#define DICT_INT32(i,s,d)		{.index = i, .subIndex = s, .dataType = uint32,	.count = 4, .data = d}

//�ֵ�ӳ�� ����-������-���ݳ���
//�޷���32λ
//�������������ַ��ֵ��0x65766173�����ѯͬ��ַ��ֵΪ 0 ʱ������ɹ���ֵΪ 1 ʱ������ʧ�ܡ�
#define MAP_INDEX(map)			((map>>16)&0xFFFF)
#define MAP_SUBINDEX(map)		((map>>8)&0xFF)
#define MAP_DATALEN(map)		((map&0xFF)/8)

#define MAP_PARM_SAVE			0x10100120		// ������������/״̬
#define PARM_SAVE_CMD			0x65766173

//�޷���16λ
#define	MAP_VEL_LIMIT			0x204B0010		// �ٶ�����rpm�����ƻ����ٶ�
#define MAP_MAX_DEGREE			0x21740010		// ����ת���Ƕȣ������ǶȾ���P069���õĳ��г�����
#define MAP_FRICTION_PER		0x21780010		// ����ٷֱ�
#define MAP_RATING_TORQUE		0x21790010		// ����ذٷֱ�(�г���) ��ֵΪ60E0��60E1�����ֵ����1000��
#define MAP_TORQUE_PMAX_INRANGE	0x60E00010		// �г����������޷�(ǧ�ֱ�)	���������谭�������˶��������Ǵ��̻��ǻ���
#define MAP_TORQUE_NMAX_INRANGE	0x60E10010		// �г��ڸ������޷�(ǧ�ֱ�)

//�޷���32λ
#define MAP_INIT_SWITCH			0x27F00120		// ��ʼ������,�˿��ظ�1���ŷ��Զ��л�������ģʽ
#define MAP_HOME_SET			0x27F00520		// ��λ�����ÿ��أ���1��MAP_HOME_DEGREE��Ϊ0�㣬
												// ���δ����MAP_HOME_DEGREE���򽫵�ǰλ����Ϊ0�㡣
#define	MAP_HOME_DEGREE			0x27F00B20		// ��λ��Ƕ����ã����ýǶȣ�Ȼ�������λ������ָ��

//�޷���32λ
#define MAP_CUR_OP_MODE			0x27F10320		// ��ǰOP״̬��Ϊ1ʱ�ɹ��������ط���ģʽ
#define MAP_IS_HOME_INIT		0x27F10420		// �����̻���λ״̬��Ϊ1ʱ����λ���
#define MAP_DECODER_VALUE		0x27F10920		// ��������Ȧ����ֵ

//�з���32λ
#define	MAP_CUR_TORQUE_PER		0x27F20120		// ��ǰ���ط��� (�ٷֱ�,��λ����0��ת����������һ������������󣬵����ֵ֮��㶨��
												// ���˳ʱ������Ǹ�����ʱ��Ϊ�������ֵΪ�����ٶ�MAP_SPRING_PER�趨��ֵ��
												// ���ҿ�����������ǲ��ǾͿ��Կ�����Ч
#define MAP_CUR_POS_INT16		0x27F20220		// ��ǰλ��,32768�û���λ����λ��0����ʱ������˳ʱ���С��
												// �����õ�����ת���Ƕ�MAP_MAX_DEGREE����ӳ��
#define MAP_CUR_DEGREE			0x27F20320		// ��ǰ�Ƕȣ���λ��0����ʱ������˳ʱ���С

//�з���32λ
#define MAP_ACTUAL_POS			0x60640020		// ʵ��λ��(�û���λ�� 0x20000 һȦ,��ʱ������˳ʱ���С�����С����ֵ)
#define MAP_ACTUAL_VLE			0x606C0020		// ʵ���ٶȣ���ֵ��+-70���ҵ�Ư��
#define MAP_TARGET_POS			0x607A0020		// Ŀ���ٶ�
#define MAP_TARGET_VEL			0x60FF0020		// Ŀ���ٶ�

//�з���16λ
#define MAP_ACTUAL_TORQUE		0x60770010		// ʵ��ת��	0.1% ������MAP_CUR_TORQUE_PER��0.1��
#define MAP_TARGET_TORQUE		0x60710010		// Ŀ��ת��

//�޷���16λ
#define MAP_CONTROL_WORD		0x60400010		// ������
#define MAP_STATUS_WORD			0x60410010		// ״̬��
#define MAP_MODE_OF_OPERATION	0x60600008		// ����ģʽ���ڷ���ʹ������֮ǰ����Ҫȷ���˶���ֵ��ʹ��֮�������ģʽ�Խ��յ�ʹ������ʱ�˶���ֵΪ׼

static	uint8_t writeSdoRes;
static	uint32_t parmSaveResul;
static	AxisConfig* motorConf = NULL;
static struct rt_semaphore semSdoReply;
struct rt_mailbox mbSlaveId;
uint32_t mbSlaveIdPool;
//static 	CO_Data* motor_od = &Maxsine_Motor_OD_Data;
//�ŷ���������������
static const net_sdo_od_t maxsineStdOd[] = {
/* TPDO���� */
#ifdef CUSTOM_SERVO
	DICT_UINT8(	0x1A00,	0x00,	0),							// дTPDO1����������Ϊ0
	DICT_UINT32(0x1A00,	0x01,	MAP_CUR_POS_INT16),         // ��ǰλ��(�û���λ)
	DICT_UINT32(0x1A00,	0x02,	MAP_CUR_TORQUE_PER),		// ��ǰ���ط���(�ٷֱ�)
	DICT_UINT8(	0x1A00,	0x00,	2),                         // дTPDO1����������Ϊ1
	DICT_UINT32(0x1800,	0x01,	0x181),                     // дTPDO1 cobid
	DICT_UINT8(	0x1800,	0x02,	TPDO_TRANS_TYPE),			// дTPDO1��������
	DICT_UINT16(0x1800,	0x05,	TPDO_TRANS_EVENT_TIME),		// дTPDO1��������
#else
	/* ����ģʽ���� */
	DICT_UINT8( 0x6060, 0x00,	DRIVE_MODE),				// ���ò���ģʽ
	
	/* TPDO���� */
	DICT_UINT8(	0x1A00,	0x00,	0),							// дTPDO1����������Ϊ0
	DICT_UINT32(0x1A00,	0x01,	MAP_ACTUAL_POS),			// ��ǰλ��(�û���λ)
#ifdef READ_SPEED
	DICT_UINT32(0x1A00,	0x02,	MAP_ACTUAL_VLE),         	// ��ǰ�ٶ�(�ٷֱ�)
	DICT_UINT8(	0x1A00,	0x00,	2),                         // дTPDO1����������Ϊ2
#else
	DICT_UINT8(	0x1A00,	0x00,	1),                         // дTPDO1����������Ϊ1
#endif
	DICT_UINT32(0x1800,	0x01,	0x181),                     // дTPDO1 cobid
	DICT_UINT8(	0x1800,	0x02,	TPDO_TRANS_TYPE),			// дTPDO1��������
	DICT_UINT16(0x1800,	0x05,	TPDO_TRANS_EVENT_TIME),		// дTPDO1��������
	
	DICT_UINT8(	0x1A01,	0x00,	0),							// дTPDO2����������Ϊ0
	DICT_UINT32(0x1A01,	0x01,	MAP_STATUS_WORD),			// ״̬��
	DICT_UINT8(	0x1A01,	0x00,	1),                         // дTPDO2����������Ϊ1
	DICT_UINT32(0x1801,	0x01,	0x281),                     // дTPDO2 cobid
	DICT_UINT8(	0x1801,	0x02,	TPDO_TRANS_TYPE),			// дTPDO2��������
	DICT_UINT16(0x1801,	0x05,	TPDO_STATUS_WORD_TIME),		// дTPDO2��������
	
	/* RPDO����	*/
	DICT_UINT8(	0x1600,	0x00,	0),							// дRPDO1����������Ϊ0
	DICT_UINT32(0x1600,	0x01,	MAP_TARGET_TORQUE),			// Ŀ��ת��
	DICT_UINT8(	0x1600,	0x00,	1),                         // дRPDO1����������Ϊ1
	DICT_UINT32(0x1400,	0x01,	0x201),                     // дRPDO1 cobid
	
	DICT_UINT8(	0x1601,	0x00,	0),							// дRPDO1����������Ϊ0
	DICT_UINT32(0x1601,	0x01,	MAP_CONTROL_WORD),			// ������
	DICT_UINT8(	0x1601,	0x00,	1),                         // дRPDO1����������Ϊ1
	DICT_UINT32(0x1401,	0x01,	0x301),                     // дRPDO1 cobid
	
//	DICT_UINT8(	0x1602,	0x00,	0),							// дRPDO1����������Ϊ0
//	DICT_UINT32(0x1602,	0x01,	MAP_TARGET_POS),			// ������
//	DICT_UINT8(	0x1602,	0x00,	1),                         // дRPDO1����������Ϊ1
//	DICT_UINT32(0x1402,	0x01,	0x401),                     // дRPDO1 cobid
#endif	

//	
//	DICT_UINT8(	0x1A02,	0x00,	0),							//дTPDO3����������Ϊ0
//	DICT_UINT32(0x1A02,	0x01,	MAP_ACTUAL_TORQUE),			//��ǰ���ط���(�ٷֱ�)
//	DICT_UINT32(0x1A02,	0x02,	MAP_CUR_DEGREE),			//��ǰ���ط���(�ٷֱ�)
//	DICT_UINT8(	0x1A02,	0x00,	2),                         //дTPDO3����������Ϊ1
//	DICT_UINT32(0x1802,	0x01,	0x381),                     //дTPDO3 cobid
//	DICT_UINT8(	0x1802,	0x02,	TPDO_TRANS_TYPE),			//дTPDO3��������
//	DICT_UINT16(0x1802,	0x05,	TPDO_TRANS_EVENT_TIME),		//дTPDO3��������
};



static local_od_t localOdParam[] = 
{
	{.index = 0x1005, .subIndex = 0x00, .data = 0x40000080, .size = 4},				// SYNC COB ID
	{.index = 0x1006, .subIndex = 0x00, .data = TPDO_SYN_CYCLE, .size = 4},			// SYNC Cycle period �˴��ĵ�λ��us
	
	
/* �����TPDOӳ�䱾��RPDO */
#ifdef CUSTOM_SERVO
	{.index = 0x1600, .subIndex = 0x01, .data = MAP_CUR_POS_INT16, .size = 4},		// RPDO1�ֵ�ӳ�䣬������TPDOһһ��Ӧ
	{.index = 0x1600, .subIndex = 0x02, .data = MAP_CUR_TORQUE_PER, .size = 4},		//
#else
	{.index = 0x1600, .subIndex = 0x00, .data = 2, .size = 1},						// RPDO1�ֵ������������TPDOһһ��Ӧ
	{.index = 0x1600, .subIndex = 0x01, .data = MAP_ACTUAL_POS, .size = 4},			// RPDO1�ֵ�ӳ�䣬������TPDOһһ��Ӧ
	{.index = 0x1600, .subIndex = 0x02, .data = MAP_ACTUAL_VLE, .size = 4},			//
	
	{.index = 0x1601, .subIndex = 0x00, .data = 1, .size = 1},						// RPDO2�ֵ������������TPDOһһ��Ӧ
	{.index = 0x1601, .subIndex = 0x01, .data = MAP_STATUS_WORD, .size = 4},		// RPDO2�ֵ�ӳ�䣬������TPDOһһ��Ӧ
	
	/* ����TPDOӳ�������RPDO */
	{.index = 0x1A00, .subIndex = 0x00, .data = 0x00, .size = 4},
	{.index = 0x1A00, .subIndex = 0x01, .data = MAP_TARGET_TORQUE, .size = 4},		//
	{.index = 0x1A00, .subIndex = 0x00, .data = 0x01, .size = 4},	
	{.index = 0x1A01, .subIndex = 0x00, .data = 0x00, .size = 4},
	{.index = 0x1A01, .subIndex = 0x01, .data = MAP_CONTROL_WORD, .size = 4},		//
	{.index = 0x1A01, .subIndex = 0x00, .data = 0x01, .size = 4},
//	{.index = 0x1A02, .subIndex = 0x00, .data = 0x00, .size = 4},
//	{.index = 0x1A02, .subIndex = 0x01, .data = MAP_TARGET_POS, .size = 4},		//
//	{.index = 0x1A02, .subIndex = 0x00, .data = 0x01, .size = 4},		
#endif

};

//���ŵ����servoID��Ϊ1
maxsine_servo_t maxSineServoData = {.servoId = 1};

static void Config_maxsine_od_param(CO_Data* d,uint8_t nodeId,net_sdo_od_t* netSdo,int count);

/**
 * дSDO�Ļص�����
 *
 *
 */
static void SendNetSdoCb(CO_Data* d, uint8_t nodeId)
{
	uint32_t abortCode;
	writeSdoRes = 0xFF;
	writeSdoRes = getWriteResultNetworkDict(d, nodeId, &abortCode);
	
	if(writeSdoRes != SDO_FINISHED)
	{
		LOG_E("sdo reply error,node id = %d,abortCode = %x \r\n",nodeId,abortCode);
		return;
	}
//	
	closeSDOtransfer(d, nodeId, SDO_CLIENT);
	rt_sem_release(&semSdoReply);
	
	return;
	
}

/**
 * ��SDO�Ļص�����
 */
static void ReadNetSdoCb(CO_Data* d, uint8_t nodeId)
{
	uint32_t abortCode;
	uint32_t rcvData,rcvSize;
	uint16_t index;
	uint8_t subIndex;
	uint8_t readSdoRes;
	readSdoRes = 0xFF;
	
	//���ֵ���и�ֵ,Ŀǰֻ��һ��sdoline
	index = d->transfers[0].index;
	subIndex = d->transfers[0].subIndex;
	switch(index)
	{
#ifdef CUSTOM_SERVO
		case 0x27F1:
			switch(subIndex)
			{
				case 0x03:		//��ǰ OP ״̬
					rcvSize = 4;
					readSdoRes = getReadResultNetworkDict(d, nodeId, &rcvData,&rcvSize,&abortCode);
					*maxSineServoData.pOpMode = rcvData;
				break;

				case 0x04:		//����λ״̬
					rcvSize = 4;
					readSdoRes = getReadResultNetworkDict(d, nodeId, &rcvData,&rcvSize,&abortCode);
					*maxSineServoData.pIsHome = rcvData;
				break;
				
				default:
					
				break;
			}
		break;
#endif		
		case 0x1010:
		{
			switch(subIndex)
			{
				case 0x01:
				rcvSize = 4;
				readSdoRes = getReadResultNetworkDict(d, nodeId, &rcvData,&rcvSize,&abortCode);
				*maxSineServoData.pSaveResult = rcvData;
			}				
		}
		break;
		
		default :
			
		break;
	}
	
	if(readSdoRes != SDO_FINISHED)
	{
		LOG_E("sdo reply error,node id = %d,abortCode = %x \r\n",nodeId,abortCode);
		closeSDOtransfer(d, nodeId, SDO_CLIENT);
		return;
	}
	
	
	rt_sem_release(&semSdoReply);
	
	return;
	
}

/**
 * ��SDO���͵������ϣ����ûص���������50ms��û���յ�������ʧ��
 *
 * @param pParam Ҫ���õ�sdo����
 *
 */
static uint8_t SendNetSdo(struct net_sdo_prameter* pParam)
{
	struct net_sdo_prameter* p = pParam;
	rt_err_t result;
	writeNetworkDictCallBack(p->d,
							 p->targetID,
							 p->od.index,
							 p->od.subIndex,
							 p->od.count,
							 p->od.dataType,
							 p->pdata,
							 p->callback,0);
	
	/* �ȴ�����������ȡ�ʼ� */
	result = rt_sem_take(&semSdoReply,50);
	if(result == -RT_ETIMEOUT)	//��ʱ
	{
		LOG_E("sdo no reply,node id = %d",p->targetID);
	}
	return writeSdoRes;
}

/**
 * ��SDO���͵������ϣ���ೢ��n�Σ����û�гɹ����򷵻ش�����
 *
 * @param pParam Ҫ���õ�sdo����
 * @param n ���ԵĴ���

 */
static uint8_t SendNetSdoUntil(struct net_sdo_prameter* pParam,uint32_t n)
{
	uint8_t ret=0xFF;
	uint32_t count = n;
	while(--count > 0)
	{
		if(SendNetSdo(pParam) == SDO_FINISHED)
		{
			ret = 0;
			break;
		}
	}
	return ret;
}

/**
 * ��һ��������õ���������ȴ�����������
 *
 * @param netSdo һ����Ҫ���õ��ֵ�
 * @param count �ܹ������õ��ֵ����

 */
static void Config_maxsine_od_param(CO_Data* d,uint8_t nodeId,net_sdo_od_t* netSdo,int count)
{
	struct net_sdo_prameter parm;
	uint32_t writeData;
	int i=0;
	parm.d = d;
	parm.callback = SendNetSdoCb;
	parm.targetID = nodeId;

	for(i = 0; i < count; i++)
	{
		parm.od.index = netSdo[i].index;
		parm.od.subIndex = netSdo[i].subIndex;
		parm.od.count = netSdo[i].count;
		parm.od.dataType = netSdo[i].dataType;
		parm.pdata = (void*)&netSdo[i].data;
		//���ó���ֱ��return
		if(SendNetSdoUntil(&parm,2))
		{
			LOG_E("Config_maxsine_od failed,index = %x;subindex = %x!\r\n",netSdo[i].index,netSdo[i].subIndex);
			return;
		}
		//��ʱ5ms������һ��
		rt_thread_delay(5);
	}
		
	return;
}

static void Maxsine_Servo_Data_Init(void)
{
	maxSineServoData.pControlWord=					&Controlword;
	maxSineServoData.pStatusWord=                   &Statusword;
	maxSineServoData.pModes_of_operation=           &Modes_of_operation;
	maxSineServoData.pModes_of_operation_display=   &Modes_of_operation_display;
	maxSineServoData.pPosition_actual_value=	    &Position_actual_value;	
	maxSineServoData.pVelocity_actual_value=	    &Velocity_actual_value;	
	maxSineServoData.pTarget_torque=                &Target_torque;
	maxSineServoData.pTorque_actual_value=	        &Torque_actual_value;	
	maxSineServoData.pTarget_position=              &Target_position;
	maxSineServoData.pTarget_velocity=              &Target_velocity;
	maxSineServoData.pSupported_drive_modes=        &Supported_drive_modes;
	maxSineServoData.pPramArr0=						para_arr0;
	maxSineServoData.pPramArr1=						para_arr1;
	maxSineServoData.pPramArr2=						para_arr2;
	maxSineServoData.motorStatus = 					INIT;
	maxSineServoData.pOpMode=                  		&para_arr1[2];
	maxSineServoData.pIsHome=                  		&para_arr1[3];
	maxSineServoData.pSaveResult = 					&parmSaveResul;
}

static void shellsetpos(int argc,char** argv)
{
	int16_t pos;
	if(!maxSineServoData.isInited)
	{
		LOG_W("servo not inited!\r\n");
		return;
	}
	if(argc < 2)
	{
		LOG_W("please input position!\r\n");
		return;
	}
	else
	{
		pos = atoi(argv[1]);
	}
	*maxSineServoData.pTarget_position = pos;
	sendOnePDOevent(motor_od,TPDO_NUM_POS_VEL);
}
MSH_CMD_EXPORT(shellsetpos , set position);

uint8_t Motor_Is_Ready(void)
{
	return maxSineServoData.isInited;
}

#ifdef CUSTOM_SERVO
static rt_err_t Motor_Start(CO_Data* d,uint8_t nodeId)
{
	rt_err_t result;
	uint32_t data = 0x01;
	writeNetworkDictCallBack(d,
							 nodeId,
							 MAP_INDEX(MAP_INIT_SWITCH),
							 MAP_SUBINDEX(MAP_INIT_SWITCH),
							 MAP_DATALEN(MAP_INIT_SWITCH),
							 uint32,
							 &data,
							 SendNetSdoCb,
							 0);
	/* �ȴ�����������ȡ�ʼ� */
	result = rt_sem_take(&semSdoReply,50);
	return result;
}

static rt_err_t Motor_Status_Check(CO_Data* d,uint8_t nodeId)
{
	rt_err_t result;
	readNetworkDictCallback(d,
							nodeId,
							MAP_INDEX(MAP_CUR_OP_MODE),
							MAP_SUBINDEX(MAP_CUR_OP_MODE),
							uint32,
							ReadNetSdoCb,
							0);
	/* �ȴ�����������ȡ�ʼ� */
	result = rt_sem_take(&semSdoReply,50);
	return result;
}

static rt_err_t Motor_Home_Check(CO_Data* d,uint8_t nodeId)
{
	rt_err_t result;
	readNetworkDictCallback(d,
							 nodeId,
							 MAP_INDEX(MAP_IS_HOME_INIT),
							 MAP_SUBINDEX(MAP_IS_HOME_INIT),
							 uint32,
							 ReadNetSdoCb,
							 0);
	/* �ȴ�����������ȡ�ʼ� */
	result = rt_sem_take(&semSdoReply,50);
	return result;
}


/**
  * @brief   ���÷����̵���λ
  */
rt_err_t Motor_Set_Home(void)
{
	if(!Motor_Is_Ready())
	{
		return RT_ERROR;
	}
	rt_err_t result;
	uint32_t data = 1;
	uint32_t saveResult = 1;
	writeNetworkDictCallBack(motor_od,
							maxSineServoData.servoId,
							MAP_INDEX(MAP_HOME_SET),
							MAP_SUBINDEX(MAP_HOME_SET),
							MAP_DATALEN(MAP_HOME_SET),
							uint32,
							&data,
							SendNetSdoCb,
							0);
	/* �ȴ�����������ȡ�ʼ� */
	result = rt_sem_take(&semSdoReply,50);
	return result;
}

/**
  * @brief   �����ŷ����������浽eeprom���´��ϵ���Զ���ȡ
  *			���浽������Ҫ�п�ת�Ƕȣ�����ͻ����ٶ�
  */
uint32_t Motor_Parm_Save(void)
{
	if(!Motor_Is_Ready())
	{
		return RT_ERROR;
	}
	rt_err_t result;
	//���ͱ���ָ��
	uint32_t cmdSave = PARM_SAVE_CMD;
	writeNetworkDictCallBack(motor_od,
							maxSineServoData.servoId,
							MAP_INDEX(MAP_PARM_SAVE),
							MAP_SUBINDEX(MAP_PARM_SAVE),
							MAP_DATALEN(MAP_PARM_SAVE),
							uint32,
							&cmdSave,
							SendNetSdoCb,
							0);
	/* �ȴ�����������ȡ�ʼ� */
	result = rt_sem_take(&semSdoReply,50);
	rt_thread_delay(500);
	*maxSineServoData.pSaveResult = 1;		
	//��ȡ������,ʵ�ʽ����Maxsine_Motor_OD_Data.pSaveResult����
	readNetworkDictCallback(motor_od,
							 maxSineServoData.servoId,
							 MAP_INDEX(MAP_PARM_SAVE),
							 MAP_SUBINDEX(MAP_PARM_SAVE),
							 uint32,
							 ReadNetSdoCb,
							 0);	
	/* �ȴ�����������ȡ�ʼ� */
	result = rt_sem_take(&semSdoReply,50);
	return *maxSineServoData.pSaveResult;
}

rt_err_t Motor_Set_Rating_Torque(uint16_t t)
{
	if(!Motor_Is_Ready())
	{
		return RT_ERROR;
	}
	rt_err_t result;
	uint16_t data = t;
	writeNetworkDictCallBack(motor_od,
							maxSineServoData.servoId,
							MAP_INDEX(MAP_RATING_TORQUE),
							MAP_SUBINDEX(MAP_RATING_TORQUE),
							MAP_DATALEN(MAP_RATING_TORQUE),
							uint16,
							&data,
							SendNetSdoCb,
							0);
	/* �ȴ�����������ȡ�ʼ� */
	result = rt_sem_take(&semSdoReply,50);
	return result;
}

/**
  * @brief      ���÷����̵Ŀ�ת�Ƕ�
  * @param[in]  deg   90-1080��
  */
rt_err_t Motor_Set_Degrees(uint16_t deg)
{
	if(!Motor_Is_Ready())
	{
		return RT_ERROR;
	}
	rt_err_t result;
	uint16_t data = deg;
	CLIP(data,90,1080);
	writeNetworkDictCallBack(motor_od,
							maxSineServoData.servoId,
							MAP_INDEX(MAP_MAX_DEGREE),
							MAP_SUBINDEX(MAP_MAX_DEGREE),
							MAP_DATALEN(MAP_MAX_DEGREE),
							uint16,
							&data,
							SendNetSdoCb,
							0);
	/* �ȴ�����������ȡ�ʼ� */
	result = rt_sem_take(&semSdoReply,50);
	return result;
}

/**
  * @brief      ���÷����̵�����ٷֱ�
  * @param[in]  percent  0%-100%	��Ӧ������ 0%-1000%
  */
rt_err_t Motor_Set_Friction(uint8_t percent)
{
	if(!Motor_Is_Ready())
	{
		return RT_ERROR;
	}
	rt_err_t result;
	uint16_t data = percent*10;
	CLIP(data,0,1000);
	//��������д��ͬ����ֵ
	writeNetworkDictCallBack(motor_od,
							maxSineServoData.servoId,
							MAP_INDEX(MAP_TORQUE_PMAX_INRANGE),
							MAP_SUBINDEX(MAP_TORQUE_PMAX_INRANGE),
							MAP_DATALEN(MAP_TORQUE_PMAX_INRANGE),
							uint16,
							&data,
							SendNetSdoCb,
							0);
	/* �ȴ�����������ȡ�ʼ� */
	result = rt_sem_take(&semSdoReply,50);
	
	writeNetworkDictCallBack(motor_od,
							maxSineServoData.servoId,
							MAP_INDEX(MAP_TORQUE_NMAX_INRANGE),
							MAP_SUBINDEX(MAP_TORQUE_NMAX_INRANGE),
							MAP_DATALEN(MAP_TORQUE_NMAX_INRANGE),
							uint16,
							&data,
							SendNetSdoCb,
							0);
	/* �ȴ�����������ȡ�ʼ� */
	result = rt_sem_take(&semSdoReply,50);
	return result;
}

/**
  * @brief      ���÷����̵Ļ����ٶȰٷֱ�
  * @param[in]  percent  0%-100%	��Ӧ������ 0%-1000%
  */
rt_err_t Motor_Set_IdleSpring(uint8_t percent)
{
	if(!Motor_Is_Ready())
	{
		return RT_ERROR;
	}
	rt_err_t result;
	uint16_t data = percent*10;
	writeNetworkDictCallBack(motor_od,
							maxSineServoData.servoId,
							MAP_INDEX(MAP_VEL_LIMIT),
							MAP_SUBINDEX(MAP_VEL_LIMIT),
							MAP_DATALEN(MAP_VEL_LIMIT),
							uint16,
							&data,
							SendNetSdoCb,
							0);
	/* �ȴ�����������ȡ�ʼ� */
	result = rt_sem_take(&semSdoReply,50);
	return result;
}
#endif		//CUSTOM_SERVO
/*
 * ���÷�������
*/
void Motor_Set_TargetTorque(int16_t force)
{
//	if(maxSineServoData.motorStatus == OPERATING)
//	{
//		Target_torque = force;
//		sendOnePDOevent(motor_od,0);
//	}
	
	if(maxSineServoData.motorStatus > INITED)
	{
		Target_torque = force;
		sendOnePDOevent(motor_od,0);
	}
	
}

static void Motor_Set_TargetPos(int32_t  pos)
{
	if(maxSineServoData.motorStatus == INITED)
	{
		Target_position = pos;
		sendOnePDOevent(motor_od,2);
	}
}

static rt_err_t Motor_Set_TargetVel(int32_t  vel)
{
	rt_err_t result;
	int32_t data = vel;
	writeNetworkDictCallBack(motor_od,
							maxSineServoData.servoId,
							MAP_INDEX(MAP_TARGET_VEL),
							MAP_SUBINDEX(MAP_TARGET_VEL),
							MAP_DATALEN(MAP_TARGET_VEL),
							int32,
							&data,
							SendNetSdoCb,
							0);
	/* �ȴ�����������ȡ�ʼ� */
	result = rt_sem_take(&semSdoReply,50);
	return result;
}

void Motor_Emergency_Stop(uint8_t rs)
{
	if(rs)
	{
		maxSineServoData.motorStatus = EMERGENCY;
	}
	else
	{
		maxSineServoData.motorStatus = INIT;
	}
}

uint8_t Motor_Get_MotorStatus(void)
{
	return maxSineServoData.motorStatus;
}

int16_t Motor_Get_Torque(void)
{
	int16_t tq = 0;
#ifdef CUSTOM_SERVO
	tq = CLIP(maxSineServoData.pPramArr2[0],-100,100);
#else
	
#endif
	return tq;
}

int16_t Motor_Get_Pos(void)
{
	int16_t pos = 0;
#ifdef CUSTOM_SERVO
	int nPos = -1*maxSineServoData.pPramArr2[1];
	pos = CLIP(nPos,-32768,32767);
#else
	
#endif
	return pos;
}


void Motor_Configure_Init(AxisConfig* conf)
{
	motorConf = conf;
}

static void Motor_Std_Control(uint16_t cw)
{
	*maxSineServoData.pControlWord = cw;
	sendOnePDOevent(motor_od,1);
}

static rt_err_t Motor_Set_OperationMode(uint8_t op)
{
	rt_err_t result;
	uint8_t data = op;
	writeNetworkDictCallBack(motor_od,
							maxSineServoData.servoId,
							MAP_INDEX(MAP_MODE_OF_OPERATION),
							MAP_SUBINDEX(MAP_MODE_OF_OPERATION),
							MAP_DATALEN(MAP_MODE_OF_OPERATION),
							uint8,
							&data,
							SendNetSdoCb,
							0);
	/* �ȴ�����������ȡ�ʼ� */
	result = rt_sem_take(&semSdoReply,50);
	return result;
}



extern struct rt_semaphore semCanReady,semMotorReady;
void Motor_Thread_Entry(void* parameter)
{
	rt_err_t result;
	uint8_t* mbfromSlave = RT_NULL;
	int i;
	result = rt_mb_init(&mbSlaveId,
						"mbSlaveId",
						&mbSlaveIdPool,
						sizeof(mbSlaveIdPool)/4,
						RT_IPC_FLAG_FIFO);
	if(result != RT_EOK)
	{
		LOG_E("mb init failed!\r\n");
		return;
	}
	
	CANOpen_RegisterCB();
	// 1. ��������״̬��������ָ����ָ����servoId�Ƿ�����
	setState(motor_od,Pre_operational );
	rt_sem_take(&semCanReady,RT_WAITING_FOREVER);
	for(i=0;i<ASK_SERVO_TRY_TIMES;++i)	//�ϵ縴λ�ŷ�
	{
		masterSendNMTstateChange(motor_od,maxSineServoData.servoId,NMT_Reset_Comunication);
		if(rt_mb_recv(&mbSlaveId,(rt_ubase_t*)&mbfromSlave,ASK_SERVO_INTERVAL) == RT_EOK)
		{
			if(*mbfromSlave == maxSineServoData.servoId)
			{
				Maxsine_Servo_Data_Init();
				break;
			}
			else
			{
				LOG_D("invalid id recived");
			}
			
		}
	}
	if(i==ASK_SERVO_TRY_TIMES)
	{
		LOG_E("no servo detected!\r\n");
		return ;
	}
						
	// 2. ���������ID����SDO��TPDO��RPDO��id������ȷ
	 setNodeIdMaster(motor_od,maxSineServoData.servoId);
	
	// 3.���������ֵ����ݣ�Ϊ�������ڻ�ȡslave������׼��
	for(i=0;i<sizeof(localOdParam)/sizeof(local_od_t);i++)
	{
		writeLocalDict(motor_od, localOdParam[i].index, localOdParam[i].subIndex, &localOdParam[i].data, &localOdParam[i].size, 0);
	}
	
	// 4.��ʼ��sdoreply�ź�����������servo��OD
	result = rt_sem_init(&semSdoReply,
						"semsdo",
						0,
						RT_IPC_FLAG_FIFO);
	Motor_Set_OperationMode(CSP_MODE);
	Config_maxsine_od_param(motor_od,maxSineServoData.servoId,(net_sdo_od_t*)maxsineStdOd,sizeof(maxsineStdOd)/sizeof(net_sdo_od_t));
	
	// 5.��������Ϊ����ģʽ���������ӽڵ㿪ʼͨѶ
	setState(motor_od,Operational );
#ifdef STD_SERVO
	masterSendNMTstateChange(motor_od, maxSineServoData.servoId, NMT_Start_Node);
#endif

	// 6.����ffb��Ҫ�Ĳ���
//	masterSendNMTstateChange(localOD, maxSineServoData.servoId, NMT_Start_Node);
	maxSineServoData.motorStatus = INIT;
	while(1)
	{
#ifdef CUSTOM_SERVO
		switch(maxSineServoData.motorStatus)
		{
			case INIT:
				if(motorConf != NULL)
				{
					Motor_Set_Rating_Torque(80);
					Motor_Set_Degrees(motorConf->degrees);
					Motor_Set_Friction(motorConf->friction);
					Motor_Set_IdleSpring(motorConf->idlespring);
					//ע�⣬���ڴ˴�������㣬���ⷽ���̱�ת������λ�����ô���
					Motor_Start(motor_od,maxSineServoData.servoId);
					maxSineServoData.motorStatus = CHECKING;
				}
			break;
			
			case CHECKING:
				//�ڴ�״̬�£���Ҫ���OP�ͻ���״̬�����ȫ��Ϊ1���ʼ�����
				Motor_Status_Check(motor_od,maxSineServoData.servoId);
				Motor_Home_Check(motor_od,maxSineServoData.servoId);
				if(*maxSineServoData.pOpMode == 1 && *maxSineServoData.pIsHome == 1)
				{
					maxSineServoData.motorStatus = INITED;
					maxSineServoData.isInited = 1;
				}
			break;
				
			case INITED:
				blinkLed4(200,0xFFFF);
				rt_sem_release(&semMotorReady);
				maxSineServoData.motorStatus = OPERATING;
			break;
			
			case OPERATING:
				Motor_Status_Check(motor_od,1);
				rt_thread_delay(100);
			break;
			
			case EMERGENCY:
				
			break;
		}
#else
	switch(*maxSineServoData.pStatusWord)
	{
		case STATUS_NOT_READY:
			Motor_Std_Control(CONTROL_TO_READY);
		break;
		
		case STATUS_READY:
			Motor_Std_Control(CONTROL_TO_SWITCHED_ON);
		break;
		
		case STATUS_SWITCHED_ON:
			Motor_Std_Control(CONTROL_TO_OPERATION_EN);
			maxSineServoData.motorStatus = INITED;
		break;
		
		case STATUS_OPERATION_EN:
			
			if(maxSineServoData.motorStatus == EMERGENCY)
			{
				Motor_Std_Control(CONTROL_TO_QUICKSTOP);
			}
			else if(maxSineServoData.motorStatus < OPERATING)
			{
				rt_sem_release(&semMotorReady);
				maxSineServoData.motorStatus = OPERATING;
			}

		break;
		
		case STATUS_QUICK_STOP:

		break;
	}
#endif
		rt_thread_delay(100);
	}
}


